#include <config.h>
#include <core.h>

static void msgd_shmtx_wakeup(msgd_shmtx_t *mtx);


msgd_int_t
msgd_shmtx_create(msgd_shmtx_t *mtx, msgd_shmtx_sh_t *addr)
{
    mtx->lock = &addr->lock;

    if (mtx->spin == (msgd_uint_t) -1) {
        return MSGD_OK;
    }

    mtx->spin = 2048;


    mtx->wait = &addr->wait;

    if (sem_init(&mtx->sem, 1, 0) == -1) {
        ERROR("sem_init() failed");
    } else {
        mtx->semaphore = 1;
    }

    return MSGD_OK;
}


void
msgd_shmtx_destory(msgd_shmtx_t *mtx)
{

    if (mtx->semaphore) {
        if (sem_destroy(&mtx->sem) == -1) {
            ERROR("sem_destroy() failed");
        }
    }

}


msgd_uint_t
msgd_shmtx_trylock(msgd_shmtx_t *mtx)
{
    return (*mtx->lock == 0 && msgd_atomic_cmp_set(mtx->lock, 0, msgd_pid));
}


void
msgd_shmtx_lock(msgd_shmtx_t *mtx)
{
    msgd_uint_t         i, n;

    DEBUG("shmtx lock");

    for ( ;; ) {

        if (*mtx->lock == 0 && msgd_atomic_cmp_set(mtx->lock, 0, msgd_pid)) {
            return;
        }

        if (msgd_ncpu > 1) {

            for (n = 1; n < mtx->spin; n <<= 1) {

                for (i = 0; i < n; i++) {
                    msgd_cpu_pause();
                }

                if (*mtx->lock == 0
                    && msgd_atomic_cmp_set(mtx->lock, 0, msgd_pid))
                {
                    return;
                }
            }
        }

        if (mtx->semaphore) {
            (void) msgd_atomic_fetch_add(mtx->wait, 1);

            if (*mtx->lock == 0 && msgd_atomic_cmp_set(mtx->lock, 0, msgd_pid)) {
                return;
            }

            DEBUG("shmtx wait %u", *mtx->wait);

            while (sem_wait(&mtx->sem) == -1) {
                int  err;

                err = errno;

                if (err != EINTR) {
                    ERROR("sem_wait() failed while waiting on shmtx");
                    break;
                }

                DEBUG("shmtx awoke");
            }

            continue;
        }

        msgd_sched_yield();
    }
}


void
msgd_shmtx_unlock(msgd_shmtx_t *mtx)
{
    if (mtx->spin != (msgd_uint_t) -1) {
        DEBUG("shmtx unlock");
    }

    if (msgd_atomic_cmp_set(mtx->lock, msgd_pid, 0)) {
        msgd_shmtx_wakeup(mtx);
    }
}


msgd_uint_t
msgd_shmtx_force_unlock(msgd_shmtx_t *mtx, pid_t pid)
{
    DEBUG("shmtx forced unlock");

    if (msgd_atomic_cmp_set(mtx->lock, pid, 0)) {
        msgd_shmtx_wakeup(mtx);
        return 1;
    }

    return 0;
}


static void
msgd_shmtx_wakeup(msgd_shmtx_t *mtx)
{
    msgd_atomic_uint_t  wait;

    if (!mtx->semaphore) {
        return;
    }

    for ( ;; ) {

        wait = *mtx->wait;

        if (wait == 0) {
            return;
        }

        if (msgd_atomic_cmp_set(mtx->wait, wait, wait - 1)) {
            break;
        }
    }

    DEBUG("shmtx wake %uA", wait);

    if (sem_post(&mtx->sem) == -1) {
       ERROR("sem_post() failed while wake shmtx");
    }

}

